/*
 * main.c
 */
#include "System\F2806x_Device.h"
#include <stdio.h>
#include <string.h>
#include "Drivers\debugLED.h"
#include "Drivers\OrdOutputs.h"
#include "Drivers\PwrLEDs.h"
#include "Drivers\LEDStrips.h"
#include "Drivers\RangeAndPwrMon.h"
#include "Drivers\UARTDriverDBG.h"
#include "Drivers\PWMDriver.h"
#include "Timer.h"
#include "CANDriver.h"
#include "DataFile.h"

extern "C" void InitSysCtrl(void);

// flash support
#define FLASH 1
extern "C" void InitFlash(void);
extern "C" void MemCopy(Uint16 *SourceAddr, Uint16* SourceEndAddr, Uint16* DestAddr);
extern Uint16 RamfuncsLoadStart, RamfuncsLoadEnd, RamfuncsRunStart;

#define CAN_BASE 0x010000

// Drivers
DebugLED ledDebug;
OrdOutputs ordOutputs;
PwrLEDs pwrLEDs;
LEDStrips ledStrips;
RangeAndPwrMon rangPwrMon;
UARTDriverDBG uartDBG;
PWMDriver pwmDrv;

// Components
Timer timer;
CANDriver canDrv;
SDataFile df;

// main step time
#define STEPTIME 0.01 /* standard steptime, 100 Hz */

int main(void) 
{
	//=================================
	//	INITIALISATION - General
	//=================================
	InitSysCtrl();	// Device Life support & GPIO mux settings

	// init debug LED
	ledDebug.Init();
	// init Serial
	uartDBG.Init(57600);
	uartDBG.SendData("IO Board v1.8\n\r");
			
// Only used if running from FLASH
#ifdef FLASH		
// Copy time critical code and Flash setup code to RAM
// The  RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files. 
	MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);

// Call Flash Initialization to setup flash waitstates
// This function must reside in RAM
	InitFlash();	// Call the flash wrapper init function
#endif //(FLASH)

	// Init Components
	timer.Init();
	canDrv.Init();
	
	// Init Drivers
	ordOutputs.Init();
	pwrLEDs.Init();
	ledStrips.Init();
	rangPwrMon.Init();
	pwmDrv.Init();

	// Reset DataFile
	memset(&df, 0, sizeof(df));

	// default PWMs
	df.PWMOut1 = 1000;
	df.PWMOut2 = 1000;
	df.PWMOut3 = 1000;
	df.PWMOut4 = 1000;
	df.PWMOut5 = 1000;
	df.PWMOut6 = 1000;

	// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM

//=================================
//	Endless LOOP
//=================================
	for(;;)  //infinite loop
	{
		timer.BeginPeriod();
		df.Counter++;

		rangPwrMon.Update(); // XXX
		df.Sonar = rangPwrMon.m_Sonic;
		df.IRNear = rangPwrMon.m_IRNear;
		df.IRFar = rangPwrMon.m_IRFar;
		df.Voltage = rangPwrMon.m_Voltage;
		df.Current = rangPwrMon.m_Current;
		df.Power = df.Voltage * df.Current;
		
		// check CAN RX MBOXes
		canDrv.ReadCANMBOX(0, &df.L1Intensity);
		canDrv.ReadCANMBOX(1, &df.L2Intensity);
		canDrv.ReadCANMBOX(2, &df.L3Intensity);
		canDrv.ReadCANMBOX(3, &df.L4Intensity);
		canDrv.ReadCANMBOX(4, &df.PWRLED1);
		canDrv.ReadCANMBOX(5, &df.PWRLED2);

		if(canDrv.ReadCANMBOX(6, &df.PWMOut1) )
		{
			df.PWMCountDown = (int)(1.0f / STEPTIME); // 1 second shutdown
		}
		canDrv.ReadCANMBOX(7, &df.PWMOut2);
		canDrv.ReadCANMBOX(8, &df.PWMOut3);
		canDrv.ReadCANMBOX(9, &df.PWMOut4);
		canDrv.ReadCANMBOX(10, &df.PWMOut5);
		canDrv.ReadCANMBOX(11, &df.PWMOut6);
		canDrv.ReadCANMBOX(12, &df.OO1);
		canDrv.ReadCANMBOX(13, &df.OO2);
		canDrv.ReadCANMBOX(14, &df.OO3);				
		
		// PWM Check
		if( df.PWMOut1 < 1000 || df.PWMOut1 > 2001 ) df.PWMOut1 = 1000;
		if( df.PWMOut2 < 1000 || df.PWMOut2 > 2001 ) df.PWMOut2 = 1000;
		if( df.PWMOut3 < 1000 || df.PWMOut3 > 2001 ) df.PWMOut3 = 1000;
		if( df.PWMOut4 < 1000 || df.PWMOut4 > 2001 ) df.PWMOut4 = 1000;
		if( df.PWMOut5 < 1000 || df.PWMOut5 > 2001 ) df.PWMOut5 = 1000;
		if( df.PWMOut6 < 1000 || df.PWMOut6 > 2001 ) df.PWMOut6 = 1000;

		// PWM failsafe
		if(	df.PWMCountDown > 0 )
		{
			df.PWMCountDown--;
		}
		else
		{
			df.PWMOut1 = 1000;
			df.PWMOut2 = 1000;
			df.PWMOut3 = 1000;
			df.PWMOut4 = 1000;
			df.PWMOut5 = 1000;
			df.PWMOut6 = 1000;
		}

		// set values
		pwrLEDs.SetDuty(PwrLEDs::L1, df.PWRLED1);
		pwrLEDs.SetDuty(PwrLEDs::L2, df.PWRLED2);
		ledStrips.SetDuty(LEDStrips::L1, df.L1Intensity);
		ledStrips.SetDuty(LEDStrips::L2, df.L2Intensity);
		ledStrips.SetDuty(LEDStrips::L3, df.L3Intensity);
		ledStrips.SetDuty(LEDStrips::L4, df.L4Intensity);	

		pwmDrv.SetPWMuSec(PWMDriver::PWM1, df.PWMOut1 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM2, df.PWMOut2 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM3, df.PWMOut3 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM4, df.PWMOut4 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM5, df.PWMOut5 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM6, df.PWMOut6 );
		
		if( df.OO1 == 0) ordOutputs.Reset(OrdOutputs::O1);
		else ordOutputs.Set(OrdOutputs::O1);
		if( df.OO2 == 0) ordOutputs.Reset(OrdOutputs::O2);
		else ordOutputs.Set(OrdOutputs::O2);
		if( df.OO3 == 0) ordOutputs.Reset(OrdOutputs::O3);
		else ordOutputs.Set(OrdOutputs::O3);

		// dump all data to CAN
		// 100Hz
		canDrv.SendMBOX(20, df.Sonar);
		canDrv.SendMBOX(21, df.IRNear);
		canDrv.SendMBOX(22, df.IRFar);
		
		// 10 Hz
		if( (df.Counter % 10) == 0)
		{
			canDrv.SendMBOX(23, df.Current);
			canDrv.SendMBOX(24, df.Voltage);
			canDrv.SendMBOX(25, df.Power);
			canDrv.SendMBOX(26, df.LoopTimeMS);
			canDrv.SendMBOX(27, df.MissionTime);
			canDrv.SendMBOX(28, df.LoopTimeMSMAX);
		}
				
		// blink LED
		if( (df.Counter % 25) == 0)
		{
			ledDebug.Toggle(DebugLED::GREEN);
		}

		//-----------------------------------------------------------
		// finish loop
		df.LoopTimeMS = timer.ElapsedTime() * 1000; // get looptime (ms)
		if(df.LoopTimeMSMAX < df.LoopTimeMS) df.LoopTimeMSMAX = df.LoopTimeMS;

		while( !timer.HasElapsed(STEPTIME) )
		{
			// do something...
		};
		df.MissionTime += timer.ElapsedTime();
	}
}

// SETUP BOXES
void CANSetupBoxes(CANDriver* ptr)
{	
    // RX	
    ptr->SetupMBOX(0,  CAN_BASE+0x000100, 8, false); // L1intensity
    ptr->SetupMBOX(1,  CAN_BASE+0x000101, 8, false); // L2intensity
    ptr->SetupMBOX(2,  CAN_BASE+0x000102, 8, false); // L3intensity
    ptr->SetupMBOX(3,  CAN_BASE+0x000103, 8, false); // L4intensity
   	ptr->SetupMBOX(4,  CAN_BASE+0x000110, 8, false); // PWRLED1intensity
    ptr->SetupMBOX(5,  CAN_BASE+0x000111, 8, false); // PWRLED1intensity
	ptr->SetupMBOX(6,  CAN_BASE+0x000120, 8, false); // PWM1
	ptr->SetupMBOX(7,  CAN_BASE+0x000121, 8, false); // PWM2
	ptr->SetupMBOX(8,  CAN_BASE+0x000122, 8, false); // PWM3
	ptr->SetupMBOX(9,  CAN_BASE+0x000123, 8, false); // PWM4
	ptr->SetupMBOX(10,  CAN_BASE+0x000124, 8, false);// PWM5
	ptr->SetupMBOX(11,  CAN_BASE+0x000125, 8, false);// PWM6
	ptr->SetupMBOX(12,  CAN_BASE+0x000130, 8, false); // OO1
	ptr->SetupMBOX(13,  CAN_BASE+0x000131, 8, false); // OO2
	ptr->SetupMBOX(14,  CAN_BASE+0x000132, 8, false); // OO3
	
	// TX
	ptr->SetupMBOX(20,  CAN_BASE+0x000300, 8, true); // sonar
	ptr->SetupMBOX(21,  CAN_BASE+0x000301, 8, true); // ir-near
	ptr->SetupMBOX(22,  CAN_BASE+0x000302, 8, true); // ir-far
	ptr->SetupMBOX(23,  CAN_BASE+0x000310, 8, true); // current
	ptr->SetupMBOX(24,  CAN_BASE+0x000311, 8, true); // voltage
	ptr->SetupMBOX(25,  CAN_BASE+0x000312, 8, true); // power
	ptr->SetupMBOX(26,  CAN_BASE+0x000350, 8, true); // looptime
	ptr->SetupMBOX(27,  CAN_BASE+0x000351, 8, true); // totaltime
	ptr->SetupMBOX(28,  CAN_BASE+0x000352, 8, true); // totaltimeMAX
}
